%author: wxj233
%time: 2023.8.31 12:00
%function: 用于拓展目标追踪研究
clc;
clear;
close all;


% [1时间戳, 2id, 3x, 4y, 5vr, 6rcs, 7vx, 8vy, 9frame_num, 10Longitude, 
% 11Latitude, 12Altitude, 13parking, 14retrograde, 15overspeed, 16D, 17x_diff, 18y_diff]
tracks = load("tracks.mat").tracks;
data = SourceData('RadarData.csv');  % 数据预处理对象
for t = [52, 97, 243, 288, 449, 490, 540, 606, 728, 779, 895, 930, 947, 1056, 1252, 1257, 1290, 1334, 1363, 1599, 1617,1697, 1894, 1924, 1916, 1929, 1958, 1960]
for track = tracks
    % 52、197、243、288、449、490、540、606、728、779、895、930、947、1056(很大的车)、1252、1257、1290、1334、1363、1599、1617、
    % 1697、1894、1924、1916、1929、1958、1960。(以上经过人工筛选大概率是大车)
    % 626、1096、1247、1853、1901、1922也比较特别，可能是两辆车出现了遮挡的情况或者是多径反射
    % 433、509比较特别
    if track.ID == 197  % 查询ID为20的轨迹信息
        break;
    end
end

track.points = [track.points(:,1)-1688524925.689, track.points(:,2:18)];
rcs = [];
for i = 1:1:size(track.points, 1)
    figure(1);
    plot([8.5, 8.5]-i*14.4, [0, 500])
    hold on;

    p = track.points(i, :);

    frameNum = p(9);
    id = p(2);
    if id == 0
        continue;
    end
    frame = data.getFrame(frameNum);
    frame = [frame(:,1)-1688524925.689, frame(:,2:6)];

    movingPoints = frame((frame(:,5)~=0)&(frame(:,5)<40)&(frame(:,6)>-2)&(frame(:,4)>-6.5)&(frame(:,4)<8.5),:);
    [~, labels] = data.getEquivalentPoint(movingPoints, 7, 2, 0.2, 1, 10);  % 单帧的等效点,动态点聚类
    idl = find(movingPoints(:,2) == id);  % 获取这个id的点的索引
    label = labels(idl);  % 标签中的顺序与原点的顺序一致
    cluster = [];
    if label == -1
        cluster = [cluster; movingPoints(idl, :)];
    else
        cluster = [cluster; movingPoints(labels(:, 1) == label, :)];
    end

    gscatter(cluster(:,4)-(i-1)*14.4, cluster(:,3), cluster(:, 5));
    
    axis([-4000 8.5 0 500]);
    title('Target point cloud sequence');
    ylabel("distance(m)");
    xlabel("Horizontal translation distance of target point cloud(m)");
    legend;
    set(gca,'XDir','reverse'); 
    grid on;
    hold on;
    
end
end














